#ifndef _MPCSOLVER_H_
#define _MPCSOLVER_H_

#include <iostream>
#include <string>
#include "all.h"
#include <qpOASES.hpp>



// using namespace qpOASES; // contain class "Matrix",ambigurous with Eigen;
using namespace std;
using namespace Eigen;

class MPCsolver
{
private:
    Matrix<double, 13 * PREDICTION_HORIZON, 13> A_qp_;                             // Aqp(13h*13)
    Matrix<double, 13 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON> B_qp_;        // Bqp(13h*12)
    DiagonalMatrix<double, 13 * PREDICTION_HORIZON, 13 * PREDICTION_HORIZON> Q_;   // Q(13h*13h)   NOTE: Q_ is diagonal matrix
    DiagonalMatrix<double, 12 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON> R_;   // R(12h*12h)   NOTE: R_ is diagonal matrix
    Vector<double, 13 * PREDICTION_HORIZON> D_;                                    // D(13h*1)
    Vector<double, 13> x_0_;                                                       // x_0(13*1)
    Matrix<double, 12 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON, RowMajor> H_; // H(12h*12h),  NOTE: row major storage
    Vector<double, 12 * PREDICTION_HORIZON> g_;                                    // g(13h*1)
    Vector<double, 20 * PREDICTION_HORIZON> c_lb_;                                 // clb(20h*1)
    Vector<double, 20 * PREDICTION_HORIZON> c_ub_;                                 // cub(20h*1)
    Matrix<double, 20 * PREDICTION_HORIZON, 20 * PREDICTION_HORIZON, RowMajor> C_; // C(20h*20h),  NOTE: row major storage
    Vector<double, 12 * PREDICTION_HORIZON> U_;                                    // U(12h*1)

public:
    MPCsolver(/* args */);
    ~MPCsolver();

    void setD_and_x0(const Vector<double, 13 * PREDICTION_HORIZON> &vecD,
                     const Vector<double, 13> &x_0);

    void calA_qp_and_B_qp(const Vector<double, PREDICTION_HORIZON> &psi_k_d,
                          const Matrix<double, 3 * PREDICTION_HORIZON, 3> &r_k_ix);

    void calH_and_g();
    void qpSolveU();
};

#endif
